
#include "hardware.h"
#include "sys.h"
#include "macros.h"
/* #include "LPC_UTIL_DEFS.h" */
/* #include "sys_config.h" */
/* #include "SysClock.h" */
#include "vcom.h"
#include "debug.h"

/******************************************************************************
 *
 * Function Name: lowInit()
 *
 * Description:
 *    This function starts up the PLL then sets up the GPIO pins before
 *    waiting for the PLL to lock.  It finally engages the PLL and
 *    returns
 *
 * Calling Sequence: 
 *    void
 *
 * Returns:
 *    void
 * partily based on code for NXP example collection
 *****************************************************************************/

typedef void (*int_func)(void);

//----------------------------------------------------------------------------
void cpu_irq_handler(void);


void cpu_irq_handler(void)
{
   int_func ifunc;
   ifunc = (int_func)VICVectAddr;
   if(ifunc != NULL)
      (*ifunc)();
}

void wdt_reload (void )
{
	WDFEED = 0xAA;
	WDFEED = 0x55;
}

void wdt_init( unsigned int Timeout )
{
	WDCLKSEL = 0;					  /* 00 Selects the Internal RC oscillator as the Watchdog clock source */
	WDMOD = 0x03;					  /* Watchdog reset mode: operate with the Watchdog interrupt and WDRESET enabled. */
	WDTC = (Timeout*1000000UL);  /* IRC oscillator F=4Mhz, after divider 1Mhz hence 1 000 000 cycle is 1Sec */
	WDFEED = 0xAA;
	WDFEED = 0x55;
}


void sys_reset ( void )
{
	vcom_connect(FALSE);

	PIN_MODEM_POWER = FALSE;
	PIN_MODEM_POWER = FALSE;

	interrupts_disable();

	DBG("============== RESET ============="); //Dima
	
	WDTC = 2000000UL;				  /* 2.000.000  - one second */
	WDFEED = 0xAA;
	WDFEED = 0x55;

	for(;;);							  /* wait for reset */
}


static void configPLL(void)
{
	unsigned long readback;

	// check if PLL connected, disconnect if yes
	if ( PLLSTAT & PLLSTAT_PLLC ) 
	{
		PLLCON = PLLCON_PLLE;       /* Enable PLL, disconnected ( PLLC = 0 )*/
		PLLFEED = 0xaa;
		PLLFEED = 0x55;
	}

	PLLCON  = 0;        /* Disable PLL, disconnected */
	PLLFEED = 0xaa;
	PLLFEED = 0x55;

	SCS |= SCS_OSCEN;   /* Enable main OSC, SCS Man p.28 */

	while( !( SCS & SCS_OSCSTAT ) ) 
	{
		;	/* Wait until main OSC is usable */
	}

	CLKSRCSEL = CLKSRC_MAIN_OSC;   /* select main OSC as the PLL clock source */

	PLLCFG = PLLCFG_MSEL | PLLCFG_NSEL;
	PLLFEED = 0xaa;
	PLLFEED = 0x55;
	
	PLLCON = PLLCON_PLLE;       /* Enable PLL, disconnected ( PLLC = 0 ) */
	PLLFEED = 0xaa;
	PLLFEED = 0x55;
	
	CCLKCFG = CCLKCFG_CCLKSEL_VAL;     /* Set clock divider, Manual p.45 */

/* #if USE_USB */
/* 	USBCLKCFG = USBCLK_CONFIG_VAL; /\* usbclk = 288 MHz/6 = 48 MHz *\/ */
/* #endif */



//	return ;


	while ( ( PLLSTAT & PLLSTAT_PLOCK ) == 0 )  
	{
		; /* Check lock bit status */
	}
	
	readback = PLLSTAT & 0x00FF7FFF;
	while ( readback != (PLLCFG_MSEL | PLLCFG_NSEL) )
	{
		; // stall - invalid readback
	}
	
	PLLCON = ( PLLCON_PLLE | PLLCON_PLLC );  /* enable and connect */
	PLLFEED = 0xaa;
	PLLFEED = 0x55;
	while ( ((PLLSTAT & PLLSTAT_PLLC) == 0) ) 
	{
		;  /* Check connect bit status, wait for connect */
	}
}

static void lowInit(void)
{

/* #if USE_USB */
/* 	PCONP |= 0x80000000;		/\* Turn On USB PCLK *\/ */
/* #endif */

	configPLL();
	
	// setup & enable the MAM
	MAMCR = MAMCR_OFF;
#if CCLK < 20000000
	MAMTIM = 1;
#elif CCLK < 40000000
	MAMTIM = 2;
#else
	MAMTIM = 3;
#endif
	MAMCR = MAMCR_FULL;
	
	// set the peripheral bus speed
	// value computed from sys.h
	
#if PBSD == 4
	PCLKSEL0 = 0x00000000;	/* PCLK is 1/4 CCLK */
	PCLKSEL1 = 0x00000000;
#elif PBSD == 2
	PCLKSEL0 = 0xAAAAAAAA;	/* PCLK is 1/2 CCLK */
	PCLKSEL1 = 0xAAAAAAAA;
#elif PBSD == 1
	PCLKSEL0 = 0x55555555;	/* PCLK is the same as CCLK */
	PCLKSEL1 = 0x55555555;
#else
#error invalid p-clock divider
#endif

	return;
}

/******************************************************************************
 *
 * Function Name: sysInit()
 *
 * Description:
 *    This function is responsible for initializing the program
 *    specific hardware
 *
 * Calling Sequence: 
 *    void
 *
 * Returns:
 *    void
 *
 *****************************************************************************/
void sysInit(void)
{
	unsigned long i = 0;
	volatile unsigned long *vect_addr, *vect_prio;
	
	lowInit();                            // setup clocks and processor port pins

	// set the interrupt controller defaults
#if defined(RAM_RUN)
	MEMMAP = MEMMAP_SRAM;                 // map interrupt vectors space into SRAM
#elif defined(ROM_RUN)
	MEMMAP = MEMMAP_FLASH;                // map interrupt vectors space into FLASH
#else
#error RUN_MODE not defined!
#endif
	
	/* initialize VIC */
	VICIntEnClr  = 0xffffffff;
	VICVectAddr  = 0x00000000;
	VICIntSelect = 0x00000000; /* all IRQ */
	
	/* set all the vector and vector control register to 0 */
	for ( i = 0; i < 32; i++ ) 
	{
		vect_addr = (unsigned long *)(VIC_BASE_ADDR + VECT_ADDR_INDEX + i*4);
		vect_prio = (unsigned long *)(VIC_BASE_ADDR + VECT_PRIO_INDEX + i*4);
		*vect_addr = 0x00000000;
		*vect_prio = 0x0000000F;
	}

	// non-existing on LPC23xx/24xx:  VICDefVectAddr = (unsigned long)reset;     // point unvectored IRQs to reset()
	
	//  wdtInit();                         // initialize the watchdog timer
//	initSysTime();                        // initialize the system timer

	
	SCS |= (1UL<<0); // set GPIOM in SCS for fast IO
	
	
	FIO0DIR = 0;
	FIO1DIR = 0;
	FIO2DIR = 0;
	FIO3DIR = 0;
	FIO4DIR = 0;
	
	FIO0PIN = 0;
	FIO1PIN = 0;
	FIO2PIN = 0;
	FIO3PIN = 0;
	FIO4PIN = 0;
		
	PINSEL0 = 0;
	PINSEL1 = 0;
	PINSEL2 = 0;
	PINSEL3 = 0;
	PINSEL4 = 0;
	PINSEL5 = 0;
	PINSEL6 = 0;
	PINSEL7 = 0;
	PINSEL8 = 0;
	PINSEL9 = 0;
	PINSEL10 = 0;

	PINMODE0 = 0;
	PINMODE1 = 0;
	PINMODE2 = 0;
	PINMODE3 = 0;
	PINMODE4 = 0;
	PINMODE5 = 0;
	PINMODE6 = 0;
	PINMODE7 = 0;
	PINMODE8 = 0;
	PINMODE9 = 0;

/* 
00 P0.00 pin has a pull-up resistor enabled.
01 Reserved. This value should not be used.
10 P0.00 pin has neither pull-up nor pull-down.
11 P0.00 has a pull-down resistor enabled.
*/

	/* PINMODE3 = 0x0000000F;		  /\* PORT_1_PIN.PIN_16; PORT_1_PIN.PIN_17 *\/ */
	/* PINMODE9 = 0x0F000000;		  /\* PORT_4_PIN.PIN_28; PORT_4_PIN.PIN_29 *\/ */

}

